#include <arch.h>
#include <asm_macros.h>

.global entrypoint
.global _barrier

func entrypoint
    /* Check if we are starting in HYP mode */
    mrs r0, cpsr
    and r0, r0, #MODE32_MASK
    cmp r0, #MODE32_hyp
    bne .L__EL1_Reset_Handler

    /* Init HSCTLR see Armv8-R AArch32 architecture profile */
    ldr r0, =(HSCTLR_RES1 | SCTLR_I_BIT | SCTLR_C_BIT)
    mcr p15, 4, r0, c1, c0, 0

#if defined(CONFIG_AARCH32_ARMV8_R)
    /* Init HACTLR: Enable EL1 access to all IMP DEF registers */
#endif

    /* Go to SVC mode */
    mrs r0, cpsr
    bic r0, #MODE32_MASK
    orr r0, #MODE32_svc
    msr spsr_hyp, r0

    ldr r0, =.L__EL1_Reset_Handler
    msr elr_hyp, r0
    dsb
    isb
    eret

.L__EL1_Reset_Handler:
    /*
     * Initialise CPU registers to a defined state if the processor is
     * configured as Dual-redundant Core Lock-step (DCLS). This is required
     * for state convergence of the two parallel executing cores.
     */

    /* Common and SVC mode registers */
    mov r0,  #0
    mov r1,  #0
    mov r2,  #0
    mov r3,  #0
    mov r4,  #0
    mov r5,  #0
    mov r6,  #0
    mov r7,  #0
    mov r8,  #0
    mov r9,  #0
    mov r10, #0
    mov r11, #0
    mov r12, #0
    mov r13, #0         /* r13_svc */
    mov r14, #0         /* r14_svc */
    mrs r0,  cpsr
    msr spsr_cxsf, r0   /* spsr_svc */

    /* FIQ mode registers */
    cps #MODE32_fiq
    mov r8,  #0         /* r8_fiq */
    mov r9,  #0         /* r9_fiq */
    mov r10, #0         /* r10_fiq */
    mov r11, #0         /* r11_fiq */
    mov r12, #0         /* r12_fiq */
    mov r13, #0         /* r13_fiq */
    mov r14, #0         /* r14_fiq */
    mrs r0,  cpsr
    msr spsr_cxsf, r0   /* spsr_fiq */

    /* IRQ mode registers */
    cps #MODE32_irq
    mov r13, #0         /* r13_irq */
    mov r14, #0         /* r14_irq */
    mrs r0,  cpsr
    msr spsr_cxsf, r0   /* spsr_irq */

    /* ABT mode registers */
    cps #MODE32_abt
    mov r13, #0         /* r13_abt */
    mov r14, #0         /* r14_abt */
    mrs r0,  cpsr
    msr spsr_cxsf, r0   /* spsr_abt */

    /* UND mode registers */
    cps #MODE32_und
    mov r13, #0         /* r13_und */
    mov r14, #0         /* r14_und */
    mrs r0,  cpsr
    msr spsr_cxsf, r0   /* spsr_und */

    /* SYS mode registers */
    cps #MODE32_sys
    mov r13, #0         /* r13_sys */
    mov r14, #0         /* r14_sys */

    /*
     * Initialize vector base address
     */
    ldr r0, =vector_table
    stcopr r0, VBAR
    isb

    ldcopr  r0, MPIDR
    bl  plat_get_core_pos
    /* r0 holds CPU number */
    cmp r0, #0x0
    bne .L__secondarys_wait

    /*  zero bss */
    ldr r0, =__bss_start
    ldr r1, =__bss_size
    bl  bzero

    b   .L__stack_setup

.L__secondarys_wait:
    /* wait fot the primary cpu to finish up global setting */
    wfe
    ldr r1, literal_for_barrier
    ldr r4, [r1]
    cmp r4, #1
    blt .L__secondarys_wait

.L__stack_setup:
    ldcopr  r0, MPIDR
    bl  plat_set_stack

    /*
     * start run c code
     */
    b   _init

.align 3
literal_for_barrier:
    .word  _barrier

endfunc entrypoint

.data
.align 3
/*
 * barrier is used to minimal synchronization in boot
 * other cores wait for primary to set it
 */
_barrier: .8byte 0
